#include "core.h"
#include "string.h"

#define EVENTS_SIZE     32
#define EVENTS_SIZE_MSK 0x1F
#define TIMER_EVENTS_SIZE       32
#define TIMER_EVENTS_SIZE_MSK   0x1F

volatile uint8_t fsm_state_current;
static  event_t events[EVENTS_SIZE];
static  event_t timer_events[TIMER_EVENTS_SIZE];
static volatile uint8_t events_rd_ptr;
static volatile uint8_t events_wr_ptr;
static volatile uint8_t timer_events_rd_ptr;
static volatile uint8_t timer_events_wr_ptr;

//--------------------------------------------------------------------------

void fsm_remove_timer_event(uint32_t event_id)
{
    uint8_t register curr_ptr = timer_events_rd_ptr;

    while (curr_ptr != timer_events_wr_ptr)
    {
#define P (timer_events[curr_ptr])
        if (P.event_id == event_id)
        {
            P.event_handler = NULL;            
        }
#undef P
        curr_ptr = ((curr_ptr + 1) & TIMER_EVENTS_SIZE_MSK);
    }
}
//-------------------------------------------------------------------------------------
static void dispatch_timer_events(void)
{
    uint32_t event_id, event_data;
    fsm_event_handler_t event_handler;
    uint8_t register curr_ptr;

    while (timer_events_wr_ptr != timer_events_rd_ptr)
    {
		__disable_irq();
        curr_ptr = timer_events_rd_ptr;
        timer_events_rd_ptr = ((timer_events_rd_ptr + 1) & TIMER_EVENTS_SIZE_MSK);

#define P (timer_events[curr_ptr])
        event_id = P.event_id;
        event_data = P.event_data;
        event_handler = P.event_handler;
#undef P
		__enable_irq();
		if(event_handler)
			event_handler(event_id, event_data);
    }
}


void fsm_event_dispatch(void)
{
    uint32_t event_id, event_data;
    fsm_event_handler_t event_handler;
    uint8_t register curr_ptr;

    while (events_wr_ptr !=events_rd_ptr || timer_events_wr_ptr != timer_events_rd_ptr)
    {
        dispatch_timer_events();
        if (events_wr_ptr != events_rd_ptr)
        {
			__disable_irq();
            curr_ptr = events_rd_ptr;
            events_rd_ptr = ((events_rd_ptr + 1) & EVENTS_SIZE_MSK);
#define P (events[curr_ptr])
            event_id = P.event_id;
            event_data = P.event_data;
            event_handler = P.event_handler;
#undef P
			__enable_irq();
			if(event_handler)
				event_handler(event_id, event_data);
        }
    }
}
//------------------------------------------------------------------------------------------------
void fsm_event_schedule_timer_events(uint32_t event_id,
                                         uint32_t event_data,
                                         fsm_event_handler_t event_handler)
{
    uint8_t register next_ptr;
    uint8_t register curr_ptr = 0xFF;

	__disable_irq();
    next_ptr = ((timer_events_wr_ptr + 1) & TIMER_EVENTS_SIZE_MSK);
    if (next_ptr != timer_events_rd_ptr)
    {
        curr_ptr = timer_events_wr_ptr;
        timer_events_wr_ptr = next_ptr;
    }
	__enable_irq();

    if (curr_ptr != 0xFF)
    {
        timer_events[curr_ptr].event_id = event_id;
        timer_events[curr_ptr].event_data = event_data;
        timer_events[curr_ptr].event_handler = event_handler;
    }
}

void fsm_event_schedule(uint32_t event_id,
                            uint32_t event_data,
                            fsm_event_handler_t event_handler)
{
    uint8_t register next_ptr;
    uint8_t register curr_ptr = 0xFF;

	__disable_irq();
    next_ptr = ((events_wr_ptr + 1) & EVENTS_SIZE_MSK);
    if (next_ptr != events_rd_ptr)
    {
        curr_ptr = events_wr_ptr;
        events_wr_ptr = next_ptr;
    }
	__enable_irq();

    if (curr_ptr != 0xFF)
    {
        events[curr_ptr].event_id = event_id;
        events[curr_ptr].event_data = event_data;
        events[curr_ptr].event_handler = event_handler;
    }
}

//------------------------------------------------------------------------------------------------

void fsm_init(void)
{
    memset(events, 0, sizeof(events));
    memset(timer_events, 0, sizeof(timer_events));
    events_rd_ptr = 0;
    events_wr_ptr = 0;
    timer_events_rd_ptr = 0;
    timer_events_wr_ptr = 0;    
}
